#include "Foc_task.h"

Motor_t Motor;

void Foc_Task_Init()
{
    // Encoder
    FOC_HALL_Init(&Motor.Motor_Cur.BLDC_Data.FOC_HALL, 4000, 20000, 7);
    FOC_HFI_Init(&Motor.Motor_Cur.BLDC_Data.FOC_HFI, 0.5f, 0.1f, 20000, 10000, 7);
    FOC_SMO_Init(&Motor.Motor_Cur.BLDC_Data.FOC_SMO, R / 2, L / 2, 0.00005f, 7, 100000);
    FOC_VF_Init(&Motor.Motor_Cur.BLDC_Data.FOC_VF, 7, 0.00005f);

    // PID
    const float PID_Q[3] = {1, 1, 0};
    PID_Init(&Motor.Motor_Cur.Pid_Q, PID_POSITION, PID_Q, 2000, 500);
    const float PID_D[3] = {1, 1, 0};
    PID_Init(&Motor.Motor_Cur.Pid_D, PID_POSITION, PID_D, 2000, 500);
    const float PID_S[3] = {1, 1, 0};
    PID_Init(&Motor.Motor_App.Pid_S, PID_POSITION, PID_S, 2000, 500);
    const float PID_P[3] = {1, 1, 0};
    PID_Init(&Motor.Motor_App.Pid_S, PID_POSITION, PID_P, 2000, 500);
}

void USER_ADC_Injected_Callback()
{
    FOC_Current(&Motor.Motor_Cur, 0, 1);
}

void USER_TIM3_Callback()
{
    FOC_HALL_CC_IRQ_Handler(&Motor.Motor_Cur.BLDC_Data.FOC_HALL);
}
